#include "model.h"

model::model()
{
    _targetCloud.reset (new pcl::PointCloud<pcl::PointXYZRGB>);
    loadTargetCloud();
    _sourceCloud.reset (new pcl::PointCloud<pcl::PointXYZRGB>);
    _registeredCloud.reset (new pcl::PointCloud<pcl::PointXYZRGB>);
}

bool model::loadSourceCloud(QString path)
{
    const std::string stdpath = path.toStdString();
    if(pcl::io::loadPCDFile(stdpath, *_sourceCloud) == 0)
    {
        vector<int> indices;
        pcl::removeNaNFromPointCloud(*_sourceCloud, *_sourceCloud, indices);
        PointIndicesPtr cleanCloudIndices(new PointIndices);
//        cutVolume(_sourceCloud, cleanCloudIndices,FLT_MIN,FLT_MAX,FLT_MIN,FLT_MAX,FLT_MIN,425, false);
//        copyPointCloud(*_sourceCloud, *cleanCloudIndices, *_sourceCloud);
        return true;
    }
    else return false;
}

bool model::loadTargetCloud()
{
//    QFile::copy(QString(":/clouds/target.pcd"), QString("target_temp.pcd")); // copy from resources to executable folder
    if(pcl::io::loadPCDFile("target.pcd", *_targetCloud) == 0 )
        return true;
    else return false;
}

PointCloud<pcl::PointXYZRGB>::Ptr model::getTargetCloud()
{
    return _targetCloud;
}

PointCloud<pcl::PointXYZRGB>::Ptr model::getSourceCloud()
{
    return _sourceCloud;
}

PointCloud<pcl::PointXYZRGB>::Ptr model::getRegisteredCloud()
{
    return _registeredCloud;
}

vector<PointIndices> model::clusterize(PointCloud<PointXYZRGB>::Ptr cloud, PointIndicesPtr indices, int min_size, int max_size, double tolerance, bool verbose)
{
    time_t start,end;
    if(verbose)
    {
        cout << "Clusterization..." << endl;
        time(&start);
    }
    vector<PointIndices> cluster_indices;
    EuclideanClusterExtraction<PointXYZRGB> ec;
    ec.setClusterTolerance (tolerance);
    ec.setMinClusterSize (min_size);
    ec.setMaxClusterSize (max_size);
    ec.setInputCloud (cloud);
    ec.setIndices(indices);
    ec.extract (cluster_indices);
    if(verbose)
    {
        console::print_info ("Found %zu clusters\n", cluster_indices.size ());
        time (&end);
        double diff = difftime (end,start);
        cout << "Clusterization took " << diff << " seconds" << endl;
    }

    return cluster_indices;
}

vector<PointIndices> model::clusterize(PointCloud<PointXYZRGB>::Ptr cloud, int min_size, int max_size, double tolerance, bool verbose)
{
    time_t start,end;
    if(verbose)
    {
        cout << "Clusterization..." << endl;
        time(&start);
    }
    vector<PointIndices> cluster_indices;
    EuclideanClusterExtraction<PointXYZRGB> ec;
    ec.setClusterTolerance (tolerance);
    ec.setMinClusterSize (min_size);
    ec.setMaxClusterSize (max_size);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);
    if(verbose)
    {
        console::print_info ("Found %zu clusters\n", cluster_indices.size ());
        time (&end);
        double diff = difftime (end,start);
        cout << "Clusterization took " << diff << " seconds" << endl;
    }

    return cluster_indices;
}

void model::cutVolume(PointCloud<PointXYZRGB>::Ptr cloud, PointIndicesPtr cut_indexes, float min_x, float max_x, float min_y, float max_y, float min_z, float max_z, bool verbose)
{
    time_t start,end;
    if(verbose)
    {
        time(&start);
        cout << "Cutting volume..." << endl;
    }

    PassThrough<PointXYZRGB> pass;
    PointIndicesPtr cut1ptr(new PointIndices);
    PointIndicesPtr cut2ptr(new PointIndices);
    if(min_x != FLT_MIN || max_x != FLT_MAX)
    {
        //Cut the cloud on the x coordinate
        pass.setInputCloud(cloud);
        pass.setFilterFieldName("x");
        pass.setFilterLimits (min_x, max_x); // 22-30
        pass.filter (cut1ptr->indices);
        if(verbose) console::print_info ("Cut in x\n");
    }
    if(min_y != FLT_MIN || max_y != FLT_MAX)
    {
        //Cut the cloud on the y coordinate
        pass.setInputCloud(cloud);
        if(cut1ptr->indices.size() != 0)
            pass.setIndices(cut1ptr);
        pass.setFilterFieldName("y");
        pass.setFilterLimits (min_y, max_y); // 120-240
        pass.filter (cut2ptr->indices);
        if(verbose) console::print_info ("Cut in y\n");
    }
    if(min_z != FLT_MIN || max_z != FLT_MAX)
    {
        //Cut the cloud on the z coordinate
        pass.setInputCloud(cloud);
        if(cut2ptr->indices.size() != 0)
            pass.setIndices(cut2ptr);
        else if(cut1ptr->indices.size() !=0)
            pass.setIndices(cut1ptr);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(min_z, max_z); // 408-415
        pass.filter (cut_indexes->indices);
        if(verbose) console::print_info ("Cut in z\n");
    }

    if(verbose)
    {
        time (&end);
        double diff = difftime (end,start);
        cout << "Cutting Volume took " << diff << " seconds" << endl;
    }
}

Eigen::Vector3f model::computeLinesCloud(PointCloud<PointXYZRGB>::Ptr cloud, vector<PointIndices>* clusters, PointIndicesPtr indices_lines, bool verbose)
{
    time_t start,end;
    if(verbose)
    {
        time(&start);
        cout << "Computing lines..." << endl;
    }

    vector<float> percentages;
    vector<Eigen::VectorXf> lines_coefficients;
    for (size_t i = 0; i < clusters->size (); ++i)
    {
        PointCloud<PointXYZRGB>::Ptr cluster_i_cloud (new PointCloud<PointXYZRGB>);
        copyPointCloud(*cloud, clusters->at(i), *cluster_i_cloud);
        vector<int> lineInliers;
        SampleConsensusModelLine<PointXYZRGB>::Ptr model_line(new SampleConsensusModelLine<PointXYZRGB> (cluster_i_cloud));
        RandomSampleConsensus<PointXYZRGB> ransac (model_line);
        ransac.setDistanceThreshold (1);
        ransac.computeModel();
        ransac.getInliers(lineInliers);
        Eigen::VectorXf line_coeffs;
        ransac.getModelCoefficients(line_coeffs);
        lines_coefficients.push_back(line_coeffs);

        if(verbose) console::print_info ("Cluster %zu has %zu points\n", i, clusters->at(i).indices.size() );
        float inliers = lineInliers.size();
        float total = clusters->at(i).indices.size();
        float percentage = (inliers/total)*100;
        if(verbose) console::print_info ("Line inliers in cluster %zu are %f % \n", i, percentage);
        percentages.push_back(percentage);
    }
    int first_max = 0;
    int second_max = 0;
    for (int i = 1; i < percentages.size(); i++)
    {
        if (percentages[i] > percentages[first_max])
        {
            second_max = first_max;
            first_max = i;
        }
        else if (percentages[i] > percentages[second_max])
            second_max = i;
    }
    if(verbose) console::print_info ("Line clusters are %zu and %zu\n\n", first_max, second_max);
    PointIndices indices;
    indices.indices = clusters->at(first_max).indices; // copy first vector in indices
    indices.indices.insert(indices.indices.end(), clusters->at(second_max).indices.begin(), clusters->at(second_max).indices.end()); // concatenate second vector to indices
    *indices_lines = indices;
    // Compute and return the direction of the first line (they are parallel)
    Eigen::Vector3f direction;
    direction[0] = lines_coefficients[first_max][3];
    direction[1] = lines_coefficients[first_max][4];
    direction[2] = lines_coefficients[first_max][5];

    if(verbose)
    {
        cout << "Line 1 coefficients" << endl << lines_coefficients[first_max] << endl;
        cout << "Line 2 coefficients" << endl << lines_coefficients[second_max] << endl;
        console::print_info ("Returning direction (%f , %f)\n\n", direction[0], direction[1]);
        time (&end);
        double diff = difftime (end,start);
        cout << "Computing lines took " << diff << " seconds" << endl;
    }
    return direction;
}

void model::computeCentralCloud(PointCloud<PointXYZRGB>::Ptr cloud_in, vector<PointIndices>* clusters, PointIndicesPtr indices_out,bool verbose)
{
    if(verbose) console::print_info ("Cluster %zu has %zu points\n", 0, clusters->at(0).indices.size() );
    vector<int> indices;
    indices = clusters->at(0).indices; // copy first vector in indices
    indices_out->indices = indices;
}

float model::validationScoreProximityTarget (PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target)
{
    search::KdTree<PointXYZRGB>::Ptr tree (new search::KdTree<PointXYZRGB>);
    tree->setInputCloud(source);
    float sum=0;
    for (int i=0;i<target->size();i++)
    {
        PointXYZRGB currentPoint=target->at(i);
        vector<int> indices;
        vector<float> sqrDist;
        tree->nearestKSearch(currentPoint,1,indices,sqrDist);
        sum+=sqrDist.at(0);
    }
    return sum/target->size();
}

/*
Eigen::Matrix4f model::finalTransformation (PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, bool verbose)
{
    if(verbose) cout << "Final transformation calculation..." << flush;
    time_t start,end;
    if(verbose) time (&start);
    Eigen::Matrix4f final_transformation_matrix=Eigen::Matrix4f::Identity ();
    IterativeClosestPoint<PointXYZRGB, PointXYZRGB>::Ptr registration (new IterativeClosestPoint<PointXYZRGB, PointXYZRGB>);
    registration->setInputCloud(source);
    registration->setInputTarget(target);
    registration->setRANSACOutlierRejectionThreshold (2); // def:4 DISTANZA DA CUI IGNORARE ALTRI PEZZI CHE NON FANNO PARTE DEL MODELLO
    registration->setRANSACIterations(50); // def:1000
    registration->setMaximumIterations (20); // def:2
    PointCloud<PointXYZRGB>::Ptr tempregistered (new PointCloud<PointXYZRGB>);
    registration->align(*tempregistered);
    if(verbose) if (registration->hasConverged()) cout << " Converged! ";
    final_transformation_matrix = registration->getFinalTransformation();
    if(verbose)
    {
        time (&end);
        double diff = difftime (end,start);
        cout << "Final registration took " << diff << " seconds"<< endl;
    }
    return final_transformation_matrix;
}

void model::registerWithLines(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, PointCloud<PointXYZRGB>::Ptr registered, bool verbose)
{
    time_t start, linesTime, rotationTime, translationTime, orientationTime, end;
    if(verbose)
    {
        time(&start);
        cout << "Registering clouds..." << endl;
        cout << "Computing lines..." << endl;
    }

    // compute lines in source
    PointIndicesPtr sourceCutIndices (new PointIndices);
    cutVolume(source, sourceCutIndices,FLT_MIN,FLT_MAX,FLT_MIN,FLT_MAX,370,380, false); // just cut in z putting x and y to pseudo-infinite values
    vector<PointIndices> sourceClusters = clusterize(source, sourceCutIndices, 2000, 20000, 2.0, false);
    PointIndicesPtr sourceLinesIndices (new PointIndices);
    Eigen::Vector3f sourceLinesDirection = computeLinesCloud(source, &sourceClusters, sourceLinesIndices, false);
    // Bring the vector to the 1st-2nd quadrant
    if(sourceLinesDirection[1] < 0)
        sourceLinesDirection = -sourceLinesDirection;

    // compute lines in target
    PointIndicesPtr targetCutIndices (new PointIndices);
    cutVolume(target, targetCutIndices,FLT_MIN,FLT_MAX,FLT_MIN,FLT_MAX,370,380, false);
    vector<PointIndices> targetClusters = clusterize(target, targetCutIndices, 2000, 20000, 2.0, false);
    PointIndicesPtr targetLinesIndices (new PointIndices);
    Eigen::Vector3f targetLinesDirection = computeLinesCloud(target, &targetClusters, targetLinesIndices, false);
    // bring the vector to the 1st-2nd quadrant
    if(targetLinesDirection[1] < 0)
        targetLinesDirection = -targetLinesDirection; // take the opposite vector

    if(verbose)
    {
        time (&linesTime);
        cout << "Source direction: " << endl << sourceLinesDirection << endl;
        cout << "Target direction: " << endl << targetLinesDirection << endl;
        cout << "Computing lines took " << difftime(linesTime, start) << " seconds" << endl;
        cout << "Computing Rotation" << endl;
    }

    // compute ROTATION (around the origin) -- POSSIBILMENTE MODIFICARLO AFFINCHE RUOTI ATTORNO AL CENTROIDE: tarslare centroide sull'origine, ruotare, riportare il centroide dov'era
    PointCloud<PointXYZRGB>::Ptr rotated (new PointCloud<PointXYZRGB>);
    float rotation = acos(  (sourceLinesDirection.dot(targetLinesDirection)) / (sourceLinesDirection.norm() * targetLinesDirection.norm())  );
    if(sourceLinesDirection[0] < targetLinesDirection[0])
        rotation = -rotation;
    Eigen::Affine3f rotation_transformation = getTransformation(0,0,0,0,0,rotation);

    transformPointCloud(*source, *rotated, rotation_transformation);

    if(verbose)
    {
        time (&rotationTime);
        cout << "Rotation angle is: " << rad2deg(rotation) << endl;
        cout << "Computing rotation took " << difftime(rotationTime, linesTime) << " seconds" << endl;
        cout << "Computing Translation" << endl;
    }

    // compute TRANSLATION (because it rotated around the origin, we have to bring it back)
    PointCloud<PointXYZRGB>::Ptr translated (new PointCloud<PointXYZRGB>);
    PointXYZRGB maxBeforeTranslation, minBeforeTranslation, maxTarget, minTarget;
    getMinMax3D(*rotated, minBeforeTranslation, maxBeforeTranslation);
    getMinMax3D(*target, minTarget, maxTarget);
    Eigen::Affine3f traslation_transformation = getTransformation( (maxTarget.x - maxBeforeTranslation.x), (maxTarget.y - maxBeforeTranslation.y), 0, 0, 0, 0 ); // (x, y, z, roll, pitch, yaw)
    transformPointCloud(*rotated, *translated, traslation_transformation);

    if(verbose)
    {
        time (&translationTime);
        cout << "Translation distance is x: " << (maxTarget.x - maxBeforeTranslation.x) << " y:" << (maxTarget.y - maxBeforeTranslation.y) << endl;
        cout << "Computing translation took " << difftime(translationTime, rotationTime) << " seconds" << endl;
        cout << "Verifying Orientation" << endl;
    }

    // verify ORIENTATION
    // compute central cloud in source-rotated-translated
    PointIndicesPtr translatedCentralCutIndices (new PointIndices);
    cutVolume(translated, translatedCentralCutIndices,FLT_MIN,FLT_MAX,FLT_MIN,FLT_MAX,390,395, false);
    vector<PointIndices> translatedCentralClusters = clusterize(translated, translatedCentralCutIndices, 20000, 60000, 2.0, false);
    PointIndicesPtr translatedCentralClusterIndices (new PointIndices);
    computeCentralCloud(translated, &translatedCentralClusters, translatedCentralClusterIndices, false);
    Eigen::Vector4f minCentralTranslated, maxCentralTranslated;
    getMinMax3D(*translated, *translatedCentralClusterIndices, minCentralTranslated, maxCentralTranslated);
    // compute central cloud in target
    PointIndicesPtr targetCentralCutIndices (new PointIndices);
    cutVolume(target, targetCentralCutIndices,FLT_MIN,FLT_MAX,FLT_MIN,FLT_MAX,390,395, false);
    vector<PointIndices> targetCentralClusters = clusterize(target, targetCentralCutIndices, 20000, 60000, 2.0, false);
    PointIndicesPtr targetCentralIndices (new PointIndices);
    computeCentralCloud(target, &targetCentralClusters, targetCentralIndices, false);
    Eigen::Vector4f minCentralTarget, maxCentralTarget;
    getMinMax3D(*target, *targetCentralIndices, minCentralTarget, maxCentralTarget);
    // apply right orientation (DA ELIMINARE SE SI FA LA ROTAZIONE SUL CENTROIDE)
    if( abs(minCentralTarget[0] - minCentralTranslated[0]) > 5 || abs(minCentralTarget[1] - minCentralTranslated[1]) > 5)
    {
        if(verbose) cout << "Changing Orientation" << endl;
        PointCloud<PointXYZRGB>::Ptr rotatedAndOriented (new PointCloud<PointXYZRGB>);
        transformPointCloud(*rotated, *rotatedAndOriented, getTransformation(0,0,0,0,0,M_PI)); // add a 180° rotation to put it in the right orientation
        PointXYZRGB maxRotatedAndOriented, minRotatedAndOriented;
        getMinMax3D(*rotatedAndOriented, minRotatedAndOriented, maxRotatedAndOriented);
        Eigen::Affine3f traslation_transformation = getTransformation( (maxTarget.x - maxRotatedAndOriented.x), (maxTarget.y - maxRotatedAndOriented.y), 0, 0, 0, 0 );
        transformPointCloud(*rotatedAndOriented, *registered, traslation_transformation); // translate it back to the right position
    }
    else
    {
        if(verbose) cout << "Orientation already correct" << endl;
        copyPointCloud(*translated, *registered);
    }

    if(verbose)
    {
        time (&orientationTime);
        cout << "Computing orientation took " << difftime (orientationTime,translationTime) << " seconds" << endl;
        cout << "Initial Registration took " << difftime (orientationTime,start) << " seconds" << endl;
//        float score = validationScoreProximityTarget(registered, target);
//        cout << "Initial Registration preicison is: " << score << endl;
    }

    // FINAL TRANSFORMATION based on central clusters final registration
    // compute registered cloud's central cluster cloud
    PointCloud<PointXYZRGB>::Ptr registeredCentralCluster (new PointCloud<PointXYZRGB>);
    PointIndicesPtr registeredCentralCutIndices (new PointIndices);
    cutVolume(registered, registeredCentralCutIndices,FLT_MIN,FLT_MAX,FLT_MIN,FLT_MAX,390,395, false);
    vector<PointIndices> registeredCentralClusters = clusterize(registered, registeredCentralCutIndices, 20000, 60000, 2.0, false);
    PointIndicesPtr registeredCentralIndices (new PointIndices);
    computeCentralCloud(registered, &registeredCentralClusters, registeredCentralIndices, false);
    copyPointCloud(*registered, *registeredCentralIndices, *registeredCentralCluster);
    // compute target's central cluster cloud
    PointCloud<PointXYZRGB>::Ptr targetCentralCluster (new PointCloud<PointXYZRGB>);
    copyPointCloud(*target, *targetCentralIndices, *targetCentralCluster);
    // compute final transformation
    Eigen::Matrix4f final_transformtion = finalTransformation(registeredCentralCluster, targetCentralCluster, false);
    transformPointCloud(*registered, *registered, final_transformtion);

    if(verbose)
    {
        time (&end);
        cout << "Final Registration took " << difftime(end, orientationTime) << " seconds" << endl;
        cout << "Total Registration took " << difftime(end, start) << " seconds" << endl;
//        float score = validationScoreProximityTarget(registered, target);
//        cout << "Final Registration preicison is: " << score << endl;
    }
}
*/

Eigen::Vector4f model::getPlaneCoefficients(PointCloud<PointXYZRGB>::Ptr cloud)
{
    Eigen::Vector4f coefficients;
    pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal>* ne = new pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal>();
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
    ne->setInputCloud(cloud);
    ne->setSearchMethod(tree);
    ne->setRadiusSearch(0.5); // use all neighbor points in a sphere of radius 5mm to compute every normal
    ne->compute(*normals); // compute the normals
    // calculate the average normal for the plane.
    coefficients = normals->points.at(0).getNormalVector4fMap();
    for(size_t i = 1; i < normals->points.size(); i++)
    {
        pcl::Normal currentNormal = normals->points.at(i);
        coefficients +=currentNormal.getNormalVector4fMap();
    }
    coefficients = coefficients/normals->points.size();
    return coefficients;
}

float model::getAverageZ(PointCloud<PointXYZRGB>::Ptr cloud, PointIndices indices)
{
    float averageZ = 0;
    for(int i = 0; i < indices.indices.size(); i++)
        averageZ += cloud->at(indices.indices.at(i)).z;
    averageZ = averageZ/indices.indices.size();
    return averageZ;
}

Eigen::Matrix4f model::getTransformation(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target)
{
    PointCloud<PointXYZRGB>::Ptr registerdComponentCloud (new PointCloud<PointXYZRGB>);
    // Given that the main clouds are already registered we can just perform the final registration on the clouds of our components to find the transformation.
    IterativeClosestPoint<PointXYZRGB, PointXYZRGB>::Ptr registrator (new IterativeClosestPoint<PointXYZRGB, PointXYZRGB>);
    registrator->setInputCloud(source);
    registrator->setInputTarget(target);
    registrator->setMaxCorrespondenceDistance(4); // nuovo da tunare
    registrator->setRANSACOutlierRejectionThreshold (0.05);
    registrator->setRANSACIterations(1000);
    registrator->setMaximumIterations (100); // 1st stop criterion
    registrator->setTransformationEpsilon (1e-7); // 2nd stop criterion (minumum difference between actual and previous transformation to continue)
    registrator->align(*registerdComponentCloud);
    Eigen::Matrix4f transformation = registrator->getFinalTransformation();
    return transformation;
}

bool model::checkBase(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, Eigen::Vector4f min, Eigen::Vector4f max, int minPercentage, float maxScore, bool verbose)
{
    time_t start,end;
    if(verbose)
    {
        time(&start);
        cout << "Checking base connection..." << endl;
    }

    // Compute points in the given volume
    Eigen::Vector4f min_pt=min;
    Eigen::Vector4f max_pt=max;
    vector< int >  sourceIndices, targetIndices;
    getPointsInBox<PointXYZRGB>(*source, min_pt, max_pt, sourceIndices);
    getPointsInBox<PointXYZRGB>(*target, min_pt, max_pt, targetIndices);

    // POINT NUMBER CHECK - Check by number of points in the given volume
    bool pointNumberCheckPassed;
    if(sourceIndices.size() > targetIndices.size())
        pointNumberCheckPassed = true;
    else if ( 100*sourceIndices.size()/targetIndices.size() > minPercentage)
        pointNumberCheckPassed = true;
    else pointNumberCheckPassed = false;

    // SQUIZ-FITNESS CHECK - Check by Squizzato's score on the clouds in the given volume
    bool scoreCheckPassed = false;
    float score = 0;
    PointCloud<PointXYZRGB>::Ptr sourcePlug (new PointCloud<PointXYZRGB>);
    PointCloud<PointXYZRGB>::Ptr targetPlug (new PointCloud<PointXYZRGB>);
    copyPointCloud(*source, sourceIndices, *sourcePlug);
    copyPointCloud(*target, targetIndices, *targetPlug);
    if(pointNumberCheckPassed)
    {
        score = validationScoreProximityTarget(sourcePlug, targetPlug); // sarebbe da modificare che prenda cloud+indici così anche su getPlaneCoefficients si può usare la funzione originale
        if(score < maxScore)
            scoreCheckPassed = true;
    }

    // INCLINATION CHECK - check by registering the two plug clouds
    bool inclinationCheckPassed = false;
    float angle = 0;
    if(scoreCheckPassed)
    {
        Eigen::Matrix4f transformation = getTransformation(sourcePlug, targetPlug);
        Eigen::Matrix3f rotationMatrix = transformation.topLeftCorner(3,3);
        Eigen::Vector3f eulerAngles = rotationMatrix.eulerAngles(0, 1, 2);
        Eigen::Matrix3f inclinationMatrix;
        inclinationMatrix = Eigen::AngleAxisf(eulerAngles[0], Eigen::Vector3f::UnitX()) *
                Eigen::AngleAxisf(eulerAngles[1],  Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(0.0, Eigen::Vector3f::UnitZ()); // just keep the x-y rotation
        Eigen::AngleAxisf angleAxis(inclinationMatrix); // calculate inclination from x-y rotation
        cout << "Inclination Axis : " << endl << angleAxis.axis() << endl;
        cout << "Inclination Angle [rad] : " << angleAxis.angle() << endl; // angolo di inclinazione stimato dalla registrazione
        angle = rad2deg(angleAxis.angle());
        if( angle < 4)
            inclinationCheckPassed = true;
    }

    if(verbose)
    {
        cout << "Source base points number: " << sourceIndices.size() << endl;
        cout << "Target base points number: " << targetIndices.size() << endl;
        cout << "Base points difference: " << abs((int)targetIndices.size() - (int)sourceIndices.size()) << endl;
        cout << "Base points number percentage: " << 100*sourceIndices.size()/targetIndices.size() << endl;
        if(pointNumberCheckPassed) cout << "Base points-number-check: true" << endl;
        else cout << "Base points-number-check: false" << endl;
        if(pointNumberCheckPassed) cout << "Base check score: " << score << endl;
        if(scoreCheckPassed) cout << "Base score-check: true" << endl;
        else cout << "Base score-check: false" << endl;
        if(scoreCheckPassed) cout << "Inclination difference is: " << angle << " degrees" << endl;
        if(inclinationCheckPassed) cout << "Returning: true" << endl;
        else cout << "Returning: false" << endl;
        time (&end);
        double diff = difftime (end,start);
        cout << "Checking base took " << diff << " seconds" << endl;
    }

    return inclinationCheckPassed;
}

Eigen::Vector3i model::getAverageColor(PointCloud<PointXYZRGB>::Ptr cloud, vector<int>* indices)
{
    // Copy the cloud (NON NECESSARIO, SI POTREBBE DIRETTAMENTE LEGGERE I VALORI DALLA CLOUD ORIGINALE AVENDO GLI INDICI, DA SISTEMARE)
    PointCloud<PointXYZRGB>::Ptr rgbCloud(new PointCloud<PointXYZRGB>);
    copyPointCloud(*cloud,*indices,*rgbCloud);
    // Calculate the average
    Eigen::Vector3i averageRGBColor = rgbCloud->points.at(0).getRGBVector3i();
    for(size_t i = 1; i < rgbCloud->size(); i++)
        averageRGBColor += rgbCloud->points.at(i).getRGBVector3i();
    averageRGBColor = averageRGBColor/rgbCloud->size();
    return averageRGBColor;
}

bool model::checkSide(PointCloud<PointXYZRGB>::Ptr source, PointCloud<PointXYZRGB>::Ptr target, Eigen::Vector4f min, Eigen::Vector4f max, int minPercentage, int maxColorDiff, bool verbose)
{

    time_t start,end;
    if(verbose)
    {
        time(&start);
        cout << "Checking side connection..." << endl;
    }

    // Compute points in the given volume
    Eigen::Vector4f min_pt=min;
    Eigen::Vector4f max_pt=max;
    vector< int >  sourceIndices, targetIndices;
    getPointsInBox<PointXYZRGB>(*source, min_pt, max_pt, sourceIndices);
    getPointsInBox<PointXYZRGB>(*target, min_pt, max_pt, targetIndices);

    // POINT NUMBER CHECK - Check by number of points in the given volume
    bool pointNumberCheckPassed;
    if(sourceIndices.size() > targetIndices.size())
        pointNumberCheckPassed = true;
    else if ( 100*sourceIndices.size()/targetIndices.size() > minPercentage)
        pointNumberCheckPassed = true;
    else pointNumberCheckPassed = false;

    // AVERAGE COLOR CHECK - Check by the average color of the points in the given volume
    bool colorCheckPassed = false;
    Eigen::Vector3i sourceAverageColor, targetAverageColor;
    int colorDiff;
    if(pointNumberCheckPassed)
    {
        sourceAverageColor = getAverageColor(source, &sourceIndices);
        targetAverageColor = getAverageColor(target, &targetIndices);
        colorDiff = abs(targetAverageColor[0] - sourceAverageColor[0]) + abs(targetAverageColor[1] - sourceAverageColor[1]) + abs(targetAverageColor[2] - sourceAverageColor[2]);
        if(colorDiff <= maxColorDiff)
            colorCheckPassed = true;
    }

    if(verbose)
    {
        cout << "Source side points number: " << sourceIndices.size() << endl;
        cout << "Target side points number: " << targetIndices.size() << endl;
        cout << "Side points difference: " << abs((int)targetIndices.size() - (int)sourceIndices.size()) << endl;
        cout << "Side points number percentage: " << 100*sourceIndices.size()/targetIndices.size() << endl;
        if(pointNumberCheckPassed) cout << "Side points-number-check: true" << endl;
        else cout << "Side points-number-check: false" << endl;
        if(pointNumberCheckPassed) cout << "Source average color: r:" << sourceAverageColor[0] << " g:" << sourceAverageColor[1] << " b:" << sourceAverageColor[2] << endl;
        if(pointNumberCheckPassed) cout << "Target average color: r:" << targetAverageColor[0] << " g:" << targetAverageColor[1] << " b:" << targetAverageColor[2] << endl;
        if(pointNumberCheckPassed) cout << "Color Difference: " << colorDiff << endl;
        if(colorCheckPassed) cout << "Returning: true" << endl;
        else cout << "Returning: false" << endl;
        time (&end);
        cout << "Checking side took " << difftime(end,start) << " seconds" << endl;
    }

    return colorCheckPassed;
}
